from math import sin, cos, asin, atan2, sqrt

from _global import degToRad, radToDeg, clamp
from vector import Vector3

class Quaternion(object):
	TOLERANCE = 0.00001
	
	def __init__(self, *args):
		if len(args) == 4:
			#Convert to list in case args is a tuple
			self._q = list(args)
		elif len(args) == 1:
			self._q = list(args[0])
		else:
			self._q = [0,0,0,0]
	
	def __str__(self):
		return 'Quaternion (%s, %s, %s, %s)' % (self.w, self.x, self.y, self.z)
	
	def __getitem__(self, i):
		return self._q[i]
	def __setitem__(self, i, n):
		self._q[i] = n
	
	def __eq__(self, q2):
		q = self._q
		return q[0] == q2[0] and q[1] == q2[1] and q[2] == q2[2] and q[3] == q2[3]
	
	@property
	def w(self): return self._q[0]
	@w.setter
	def w(self, w): self._q[0] = w
	@property
	def x(self): return self._q[1]
	@x.setter
	def x(self, x): self._q[1] = x
	@property
	def y(self): return self._q[2]
	@y.setter
	def y(self, y): self._q[2] = y
	@property
	def z(self): return self._q[3]
	@z.setter
	def z(self, z): self._q[3] = z
	
	def magnitudeSq(self):
		w, x, y, z = self._q
		return w**2 + x**2 + y**2 + z**2;
	def magnitude(self):
		return sqrt(self.magnitudeSq())
	
	def normalize(self):
		magSq = self.magnitudeSq()
		absMagSq = abs(magSq)
		
		if absMagSq > Quaternion.TOLERANCE and absMagSq - 1. > Quaternion.TOLERANCE:
			w, x, y, z = self._q
			mag = sqrt(magSq)
			w /= mag
			x /= mag
			y /= mag
			z /= mag
			self._q = [clamp(w, -1, 1), x, y, z]
	
	def getConjugate(self):
		w, x, y, z = self._q
		return Quaternion(w, -x, -y, -z)
	
	def __mul__(self, q2):
		w, x, y, z = self._q
		w2, x2, y2, z2 = q2
		return Quaternion(w * w2 - x * x2 - y * y2 - z * z2,
						   w * x2 + x * w2 + y * z2 - z * y2,
						   w * y2 + y * w2 + z * x2 - x * z2,
						   w * z2 + z * w2 + x * y2 - y * x2)
	
	def rotateV(self, v):
		"""Rotates vector and returns it"""
		
		resQuat = self * Quaternion(0., v.z, v.x, v.y) * self.getConjugate()
		return (Vector3(resQuat.y, resQuat.z, resQuat.x))
	
	#def fromAxis(self, v, ang):
	#	ang *= .5
	#	vn = v.copy().normalize()
	#
	#	sinAngle = sin(ang)
	#
	#	self._q = [cos(ang),
	#			   vn.x * sinAngle,
	#			   vn.y * sinAngle,
	#			   vn.z * sinAngle]
	#def getAxisAngle(self, vAxis, ang):
	#	w, x, y, z = self._q
	#	scale = sqrt(x * x + y * y + z * z);
	#	vAxis.x = x / scale;
	#	vAxis.y = y / scale;
	#	vAxis.z = z / scale;
	#	ang = acos(w) * 2.
	
	def setEulerDeg(self, v):
		return self.setEuler(v * degToRad)
		
	def setEuler(self, v):
		p = v.x / 2.	#theta
		y = v.y / 2.	#psi
		r = v.z / 2.	#phi

		sinp = sin(p)
		siny = sin(y)
		sinr = sin(r)
		cosp = cos(p)
		cosy = cos(y)
		cosr = cos(r)

		self._q = [cosr * cosp * cosy + sinr * sinp * siny,
				   sinr * cosp * cosy - cosr * sinp * siny,
				   cosr * sinp * cosy + sinr * cosp * siny,
				   cosr * cosp * siny - sinr * sinp * cosy]

		self.normalize()
		return self
	
	def getEulerDeg(self):
		return self.getEuler() * radToDeg
	
	def getEuler(self):
		"""Convert to euler angles
		http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles"""
		self.normalize()
		w, x, y, z = self._q
		theta = asin(clamp(2. * (w * y - z * x), -1., 1.)) #x
		psi = atan2(2. * (w * z + x * y), 1. - 2. * (y**2 + z**2)) #y
		phi = atan2(2. * (w * x + y * z), 1. - 2. * (x**2 + y**2)) #z
		return Vector3(theta, psi, phi)
	
	def getMatrix(self):
		"""Convert to matrix usable by OpenGL in col-major format"""
		x2 = x * x; y2 = y * y; z2 = z * z
		xy = x * y; xz = x * z; yz = y * z
		wx = w * x; wy = w * y; wz = w * z

		return Matrix4(1. - 2. * (y2 + z2), 2. * (xy - wz), 2. * (xz + wy), 0.,
						2. * (xy + wz), 1. - 2. * (x2 + z2), 2. * (yz - wx), 0.,
						2. * (xz - wy), 2. * (yz + wx), 1. - 2. * (x2 + y2), 0.,
						0., 0., 0., 1.)
	
	